#include "DirectDrivePt.h"

#include <LiquidCrystal.h>
#include <Servo.h>
#include <pt.h>
#include <ServoRate.h>

LiquidCrystal lcd( 12, 11, 5, 4, 3, 7 );

#define PIN_SERVO_THROTTLE 9
#define DEGREE_THROTTLE_LOW 10
#define RATE_THROTTLE_LOW 0
#define DEGREE_THROTTLE_HIGH 18
#define RATE_THROTTLE_HIGH 1
#define PIN_SERVO_CLUTCH 10
#define DEGREE_CLUTCH_DOWN 150
#define RATE_CLUTCH_DOWN 60
#define DEGREE_CLUTCH_MIDDLE 165
#define DEGREE_CLUTCH_UP 180
#define RATE_CLUTCH_UP 0
#define SECONDS_FULL_THROTTLE 5
#define SECONDS_IDLE 7
#define LOAD_WHEEL_MIN_RPM 50
#define BUTTON_DEBOUNCE 100

struct pt ptDriveController;
PT_THREAD( driveController( struct pt & pt ) );

int nControll = 0;

struct TsServo tsServoThrottle;
struct TsServo tsServoClutch;
struct TsMicroDelay tsMicroDelay;

struct tach
{
  unsigned long nMillisLast;
  unsigned long nMillisCurrent;
};

struct tach tachLoadWheel;
void tachInit( struct tach &tach, int nMillisLastInit, int nMillisCurrentInit )
{
  tach.nMillisLast = nMillisLastInit;
  tach.nMillisCurrent = nMillisCurrentInit;
}

void tachISR()
{
  if ( ( millis() - tachLoadWheel.nMillisCurrent ) >= BUTTON_DEBOUNCE )
  {
    tachLoadWheel.nMillisLast = tachLoadWheel.nMillisCurrent;
    tachLoadWheel.nMillisCurrent = millis();
  }
}

int tachGetRpm( struct tach &tach )
{
  unsigned long nDiff = ( tach.nMillisCurrent - tach.nMillisLast );
  return ( 60000 / nDiff );
}

void setup()
{
  lcd.begin( 16, 2 );
  lcd.print( "Initializing..." );

  attachInterrupt( 0, tachISR, FALLING );
  digitalWrite( 2, HIGH );
  tachInit( tachLoadWheel, 0, 0 );

  servoInit( tsServoThrottle, PIN_SERVO_THROTTLE, DELAY_SERVO_AIRTRONICS_94102, DEGREE_THROTTLE_LOW, 0 );
  servoInit( tsServoClutch, PIN_SERVO_CLUTCH, DELAY_SERVO_AIRTRONICS_94102, DEGREE_CLUTCH_UP, 0 );

  PT_INIT( &ptDriveController );

  delay( 5000 );
  lcd.clear();
}

void loop()
{
  threadServoController( tsServoThrottle );
  threadServoController( tsServoClutch );

  driveController( ptDriveController );
}

PT_THREAD( driveController( struct pt & pt ) )
{
  PT_BEGIN(&pt)
    ;

//    // Step one: move clutch down until driveshaft begins to touch load wheel
//    lcd.clear();
//    lcd.print( "Step One" );
//    servoSetPosition( tsServoClutch, DEGREE_CLUTCH_DOWN, RATE_CLUTCH_DOWN );
//    PT_WAIT_UNTIL( &pt, servoGetPosition(tsServoClutch) == DEGREE_CLUTCH_MIDDLE );
//
//    // Step two: begin to speed up engine while continuing to lower clutch
//    lcd.clear();
//    lcd.print( "Step Two" );
//    servoSetPosition( tsServoThrottle, DEGREE_THROTTLE_HIGH, RATE_THROTTLE_HIGH );
//    PT_WAIT_UNTIL( &pt, servoGetPosition(tsServoThrottle) == DEGREE_THROTTLE_HIGH );
//
//    // Step three: wait for a bit
//    lcd.clear();
//    lcd.print( "Step Three" );
//    PT_SPAWN( &pt, &tsMicroDelay.pt, threadMicroDelay(tsMicroDelay, SECONDS_FULL_THROTTLE * 100000) );
//
//    // Step four: raise clutch and slow down engine
//    lcd.clear();
//    lcd.print( "Step Four" );
//    servoSetPosition( tsServoClutch, DEGREE_CLUTCH_UP, RATE_CLUTCH_UP );
//    PT_WAIT_UNTIL( &pt, servoGetPosition(tsServoClutch) == DEGREE_CLUTCH_MIDDLE );
//    servoSetPosition( tsServoThrottle, DEGREE_THROTTLE_LOW, RATE_THROTTLE_LOW );
//
//    // Step five: wait until load wheel slows down
//    lcd.clear();
//    lcd.print( "Step Five" );
//    PT_WAIT_UNTIL( &pt, tachGetRpm(tachLoadWheel) <= LOAD_WHEEL_MIN_RPM );
    /*
     *
     *
     *
     *
     *
     *
     *
     */
    // Step one:  set throttle to idle, and clutch down, and wait untill those actions have been completed
    servoSetPosition( tsServoClutch, DEGREE_CLUTCH_DOWN, RATE_CLUTCH_DOWN );
    servoSetPosition( tsServoThrottle, DEGREE_THROTTLE_LOW, RATE_THROTTLE_LOW );
    PT_WAIT_UNTIL( &pt, (servoGetPosition(tsServoClutch) == DEGREE_CLUTCH_DOWN) && (servoGetPosition(tsServoThrottle) == DEGREE_THROTTLE_LOW) );

    // Step two:  wait a bit
    PT_SPAWN( &pt, &tsMicroDelay.pt, threadMicroDelay(tsMicroDelay, SECONDS_IDLE * 100000) );

    // Step three:  set throttle to high and wait until complete
    servoSetPosition( tsServoThrottle, DEGREE_THROTTLE_HIGH, ( RATE_THROTTLE_HIGH ) );
    PT_WAIT_UNTIL( &pt, servoGetPosition(tsServoThrottle) == DEGREE_THROTTLE_HIGH );

    // Step four:  wait for a bit at this sate
    PT_SPAWN( &pt, &tsMicroDelay.pt, threadMicroDelay(tsMicroDelay, SECONDS_FULL_THROTTLE * 100000) );

    // Step five:  set clutch to low, and wait to complete, then set throttle to low an wait until complete
    servoSetPosition( tsServoClutch, DEGREE_CLUTCH_UP, RATE_CLUTCH_UP );
    PT_WAIT_UNTIL( &pt, servoGetPosition(tsServoClutch) == DEGREE_THROTTLE_LOW );
    servoSetPosition( tsServoThrottle, DEGREE_THROTTLE_LOW, RATE_CLUTCH_UP );
    PT_WAIT_UNTIL( &pt, servoGetPosition(tsServoThrottle) == DEGREE_THROTTLE_LOW );

    // Step five:  wait for load wheel to slow down
    PT_WAIT_UNTIL( &pt, tachGetRpm(tachLoadWheel) <= LOAD_WHEEL_MIN_RPM );

  PT_END(&pt);
return PT_ENDED;
}

